//#include <cv.h>
//#include <highgui.h>

#include <opencv2/core/utility.hpp>
#include <opencv2/opencv.hpp>

#include <sys/shm.h>
#include <iostream>
#include <sstream>
#include <stdlib.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <fcntl.h>
#include <termios.h>
#include <errno.h>
#include <string.h>
#include <math.h>
#include <sys/time.h>
#include <iostream>
#include <algorithm>
#include <fstream>
#include <unistd.h>
#include <math.h>
#include <unistd.h>
#include <stdlib.h>
#include <getopt.h>
#include <fcntl.h>
#include <string.h>
#include <string>
#include <vector>
#include <iomanip>
#include <sstream>
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <getopt.h>
#include <string.h>
#include <pthread.h>
#include <math.h>
#include <fcntl.h>
#include <errno.h>
#include <termios.h>

#include "opencv2/core/utility.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"

#include <stdio.h>
#include <string.h>
#include <ctype.h>

using namespace cv;
using namespace std;

#include <iostream>
#include <fstream>
#include <opencv2/core/core.hpp>


using namespace std;
using namespace cv;

#define HAS_FOOD

// vertex
class Vertex
{
public:
    Vertex()
    {
        vertex.x = 0;
        vertex.y = 0;
    }
    Vertex(int x, int y)
    {
        vertex.x = x;
        vertex.y = y;
    }
    Vertex(Point2f p)
    {
        vertex = p;
    }
    void set_vertex(int x, int y)
    {
        vertex.x = x;
        vertex.y = y;
    }
    void set_vertex(Point2f p)
    {
        vertex = p;
    }
    void reset_vertex()
    {
        vertex.x = 0;
        vertex.y = 0;
        neibors.clear();
        visited = false;
        closed = false;
        g_value = 100000000;
        optimal_parent = NULL;
        food = false;
    }
    void clear_flag()
    {
        visited = false;         // whether visited
        closed = false;            // whether closed
        g_value = 100000000;             // g value = shortest path length from start to this vertex
        optimal_parent = NULL;          // parent vertex
    }
public:
    map<Vertex*,double> neibors;    // neighbor vertex and weight
    Point2f vertex;                 // vertex coordinate
    bool visited = false;         // whether visited
    bool closed = false;            // whether closed
    double g_value = 100000000;             // g value = shortest path length from start to this vertex
    Vertex* optimal_parent = NULL;          // parent vertex
    bool food = false;
};

// graph
class Graph
{
public:
    Graph()
    {
        vertices.clear();
    }

    void reset_graph()
    {
        vertices.clear();
    }

    void add_vertex(int x, int y)
    {
        Vertex v(x,y);
        vertices.push_back(v);
    }

    void add_vertex(Point2f p,bool food_flag = false)
    {
        Vertex v(p);
        v.food = food_flag;
        vertices.push_back(v);
    }

    void add_vertex(Vertex v)
    {
        vertices.push_back(v);
    }

    void add_edge(int v1_id, int v2_id, double weight)
    {
        vertices[v1_id].neibors[&vertices[v2_id]] = weight;
    }
    void add_edge(Vertex* v1, Vertex* v2, double weight)
    {
        v1->neibors[v2] = weight;
    }
    
public:
    vector<Vertex> vertices;   // vertexs
};

// A* algorithm
double find_path_in_graph(Graph& G, int start_id, int end_id, vector<Vertex*>& path)
{
    G.vertices[start_id].visited = true;
    G.vertices[start_id].g_value = 0;
    G.vertices[start_id].closed = true;
    G.vertices[start_id].optimal_parent = &G.vertices[start_id];
    for( auto it = G.vertices[start_id].neibors.begin(); it != G.vertices[start_id].neibors.end(); it++ )
    {
        // visited
        (*it).first->visited = true;
        // update optimal_parent
        (*it).first->optimal_parent = &G.vertices[start_id];
        // update g_value
        (*it).first->g_value = (*it).second;
    }

    while (!G.vertices[end_id].visited)
    {
        // update status for every vertex that has been visited
        for(int i=0;i<G.vertices.size();i++)
        {
            if(G.vertices[i].visited && !G.vertices[i].closed)
            {
                for( auto it = G.vertices[i].neibors.begin(); it != G.vertices[i].neibors.end(); it++ )
                {
                    (*it).first->visited = true;
                    // update optimal_parent and g_value
                    if((*it).first->g_value > (*it).second + G.vertices[i].g_value)
                    {
                        (*it).first->g_value = (*it).second + G.vertices[i].g_value;
                        (*it).first->optimal_parent = &G.vertices[i];
                    }
                }
                // close vertex
                G.vertices[i].closed = true;
            }
        }
    }
 
    Vertex* v = &G.vertices[end_id];
    double path_length = 0;
    while(v != &G.vertices[start_id])
    {
        path.push_back(v);
        Point2f p1 = v->vertex;
        Point2f p2 = v->optimal_parent->vertex;
        v = v->optimal_parent;
        path_length += sqrt((p1.x-p2.x)*(p1.x-p2.x)+(p1.y-p2.y)*(p1.y-p2.y));
    }
    path.push_back(&G.vertices[start_id]);

    // clear flag
    for(int i=0;i<G.vertices.size();i++)
    {
        G.vertices[i].clear_flag();
    }

    return path_length;
}

// generate random sequence of points
deque<int> generate_random_sequence(int num)
{
    deque<int> sequence;
    for(int i=0;i<num;i++)
    {
        sequence.push_back(i);
    }
    random_shuffle(sequence.begin(), sequence.end());
    return sequence;
}

// probability trigger
bool probability_trigger(double p)
{
    double r = (double)rand()/RAND_MAX;
    if(r < p)
    {
        return true;
    }
    else
    {
        return false;
    }
}

int main()
{
    // limitation of the total path length
    // 1. unity_maze 边长 10m ; TSP_map 边长 756 pixel
    // 2. agent 速度10m/s
    // 3. unity_maze 时间限制 20秒
    // 4. 对应 TSP_map 的最长行驶距离=20*756=15120(pixels)
    // 5. 共有 50 个食物，69 个路径点
    double life_duration = 15120;

    double anealing_rate = 0.9995;
    double inital_temperature = 1;

    // set random seed
    srand( (unsigned)time( NULL ) );

    cv::Mat frame0 = cv::imread("../map.png", 1);
    int width = frame0.cols;
    int height = frame0.rows;
    int border = 10;

    // generate maze bitmap
    cv::Mat frame1 = frame0.clone();
    for(int y=0;y<frame1.rows;y++)
    {
        for(int x=0;x<frame1.cols;x++)
        {
            double r = frame1.at<Vec3b>(y,x)[2];
            double g = frame1.at<Vec3b>(y,x)[1];
            double b = frame1.at<Vec3b>(y,x)[0];
            double dist = sqrt((r-115)*(r-115)+(g-75)*(g-75)+(b-69)*(b-69));
            if(dist <= 20)
            {
                frame1.at<cv::Vec3b>(y,x)[0] = 255;
                frame1.at<cv::Vec3b>(y,x)[1] = 255;
                frame1.at<cv::Vec3b>(y,x)[2] = 255;
            }
            else
            {
                frame1.at<cv::Vec3b>(y,x)[0] = 0;
                frame1.at<cv::Vec3b>(y,x)[1] = 0;
                frame1.at<cv::Vec3b>(y,x)[2] = 0;
            }
        }
    }
    // generate 50 random 2D points
    vector<Point2f> points0;
    
    points0.push_back(Point2f(width/2,height/2));
#ifdef HAS_FOOD
    for( int i = 0; i < 50; i++ )
#else
    for( int i = 0; i < 0; i++ )
#endif
    {
        // x coordinates between border and width-border
        // y coordinates between border and height-border
        double x = border + rand() % (width-border*2);
        double y = border + rand() % (height-border*2);
        while(frame1.at<cv::Vec3b>(y,x)[0] == 0)
        {
            x = border + rand() % (width-border*2);
            y = border + rand() % (height-border*2);
        }
        points0.push_back(Point2f(x,y));
    }

    Graph G;

    cv::Mat frame = frame1.clone();

    G.reset_graph();

    // convert to grayscale
    cv::Mat gray;
    cv::cvtColor(frame1, gray, cv::COLOR_BGR2GRAY);
    // dilate image frame
    cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3,3));
    cv::Mat gray1;
    // inverse the image gray
    cv::threshold(gray, gray1, 0, 255, cv::THRESH_BINARY_INV);
    for(int i=0;i<30;i++)
        cv::dilate(gray1, gray1, element);
    // find corners in gray1
    vector<cv::Point2f> corners;
    cv::goodFeaturesToTrack(gray1, corners, 200, 0.15, 30);

    // // merge gray1 and gray
    // cv::Mat gray2;
    // cv::addWeighted(gray1, 0.2, gray, 0.8, 0, gray2);

    // // draw corners on gray2
    // for(int i=0;i<corners.size();i++)
    // {
    //     cv::circle(gray2, corners[i], 10, cv::Scalar(0,0,255), 1);
    // }
    // cv::imshow("gray1", gray2);

    // merge points1 and corners
    vector<Point2f> points1;
    points1.insert(points1.end(), points0.begin(), points0.end());
    points1.insert(points1.end(), corners.begin(), corners.end());
    
    // add points to graph
    for( int i = 0; i < points0.size(); i++ )
    {
        circle( gray, points0[i], 7, Scalar(255,255,255), -1, 8);
        G.add_vertex(points0[i],true);
    }
    for( int i = 0; i < corners.size(); i++ )
    {
        circle( gray, corners[i], 7, Scalar(255,255,255), -1, 8);
#ifdef HAS_FOOD
        G.add_vertex(corners[i],false);
#else
        G.add_vertex(corners[i],true);
#endif
    }
    // add_edge between each pair of points in points1
    int line_n = 0;
    for( int i = 0; i < points1.size(); i++ )
    {
        for( int j = 0; j < points1.size(); j++ )
        {
            if(i==j)
                continue;
            // traverse the line between points1[i] and points1[j]
            Point2f p1 = points1[i];
            Point2f p2 = points1[j];
            Point2f p;
            bool edge_blocked = false;
            for( float t = 0.0; t < 1.0; t += 0.01 )
            {
                p = (1-t)*p1 + t*p2;
                // draw circle at p
                // circle( frame, p, 1, Scalar(0,0,0), -1, 1);
                if(gray.at<uchar>(p.y,p.x) == 0)
                {
                    edge_blocked = true;
                    break;
                }
            }
            if(!edge_blocked)
            {
                // line( frame, p1, p2, Scalar(0,150,0), 1, 8);
                G.add_edge(i, j, sqrt((p1.x-p2.x)*(p1.x-p2.x)+(p1.y-p2.y)*(p1.y-p2.y)));
            }
        }
    }

    // generate distance matrix for foods
    vector<vector<float> > distance_matrix;
    deque<int> foods;
    for(int i=0;i<G.vertices.size();i++)
    {
        vector<float> row;
        if(G.vertices[i].food)foods.push_back(i);
        for(int ii=0;ii<G.vertices.size();ii++)
        {
            if(i==ii)
            {
                row.push_back(0);
                continue;
            }
            // calculate minimum distance between vertices[i] and vertices[j] using A* algorithm
            int start_id = i;
            int end_id = ii;
            vector<Vertex*> path;
            float path_length = find_path_in_graph(G, start_id, end_id, path);
            row.push_back(path_length);
        }
        distance_matrix.push_back(row);
    }
    // // print distance matrix
    // for(int i=0;i<distance_matrix.size();i++)
    // {
    //     for(int j=0;j<distance_matrix[i].size();j++)
    //     {
    //         cout<<distance_matrix[i][j]<<",";
    //     }
    //     cout<<endl;
    // }

    // random_shuffle foods from element 1 to end
    random_shuffle(foods.begin()+1, foods.end());

    cv::imshow("frame", frame);
    cv::imshow("best order", frame);
    cv::waitKey(0);

    deque<int> optimal_sequence;
    double minimum_length = 1000000;
    optimal_sequence = foods;

    int current_reward_during_lifecycle = 0;
    int max_reward_during_lifecycle = 0;
    int max_reward = 0;

    for(int k=0,j=0;;k++)
    {
        cv::Mat frame2 = frame.clone();
        cv::Mat frame3 = frame.clone();
        
        // std::swap(foods[0], foods[foods.size()-1]);

        // calculate total path length of food sequence from distance matrix
        float current_path_length = 0;
        for(int i=0;i<foods.size()-1;i++)
        {
            current_path_length += distance_matrix[foods[i]][foods[i+1]];
        }

        // make a clone of foods
        deque<int> foods_clone = foods;
        // randomly select N elements from foods_clone
        int i1 = 1+rand() % (foods_clone.size()-1);
        int i2 = 1+rand() % (foods_clone.size()-1);
        
        // swap them
        std::swap(foods_clone[i1], foods_clone[i2]);

        // // calculate new path length of food sequence from distance matrix
        // float new_path_length = 0;
        // for(int i=0;i<foods_clone.size()-1;i++)
        // {
        //     new_path_length += distance_matrix[foods_clone[i]][foods_clone[i+1]];
        // }

        // if(new_path_length < current_path_length || probability_trigger(inital_temperature))
        // {
        //     foods = foods_clone;
        //     current_path_length = new_path_length;
        //     // cout<<"new path length: "<<current_path_length<<" , "<<inital_temperature<<endl;
        // }

        // if(inital_temperature < 1e-06)
        // {
        //     if(minimum_length > current_path_length)
        //     {
        //         minimum_length = current_path_length;
        //         optimal_sequence = foods;
        //     }

        //     cout<<"new path length: "<<current_path_length<<" , "<<inital_temperature<<endl;
        //     inital_temperature = 1;
        //     random_shuffle(foods.begin()+1, foods.end());
        // }

        // calculate reward of current action sequence during lifecycle
        current_path_length = 0;
        current_reward_during_lifecycle = 0;
        deque<int> current_sequence;
        for(int i=0;i<foods_clone.size()-1;i++)
        {
            if(current_path_length <= life_duration)
            {
                current_path_length += distance_matrix[foods_clone[i]][foods_clone[i+1]];
                current_reward_during_lifecycle ++;
                current_sequence.push_back(foods_clone[i]);
            }
            else
            {
                current_path_length -= distance_matrix[foods_clone[i-1]][foods_clone[i]];
                break;
            }
        }
        // conditional update
        if(current_reward_during_lifecycle > max_reward_during_lifecycle || probability_trigger(inital_temperature))
        {
            max_reward_during_lifecycle = current_reward_during_lifecycle;
            // update the best sequence
            foods = foods_clone;
        }
        if(inital_temperature < 1e-06)
        {
            if(max_reward_during_lifecycle > max_reward)
            {
                max_reward = max_reward_during_lifecycle;
                optimal_sequence = current_sequence;
            }
            cout<<"max_reward: "<<max_reward<<" , temperature: "<<inital_temperature<<" , current_path_length: "<<current_path_length<<endl;
            inital_temperature = 1;
            random_shuffle(foods.begin()+1, foods.end());
            j=0;
        }

        inital_temperature *= anealing_rate;

        // visualization
        int display_rate = 200;
        if(k%100==0)
        {
            j++;
            double r = double(j%display_rate)/(double)display_rate;

            // visualize graph
            for( int i = 0; i < G.vertices.size(); i++ )
            {
                for( auto it = G.vertices[i].neibors.begin(); it != G.vertices[i].neibors.end(); it++ )
                {
                    Point2f p1 = G.vertices[i].vertex;
                    Point2f p2 = (*it).first->vertex;
                    line( frame2, p1, p2, Scalar(0,150,0), 1, 8);
                    // line( frame3, p1, p2, Scalar(0,150,0), 1, 8);
                }
            }
            
            // draw foods sequently
            for(int i=0;i<foods.size()-1;i++)
            {
                int food_id = foods[i];
                int next_food_id = foods[i+1];
                Point2f food_pos = G.vertices[food_id].vertex;
                Point2f next_food_pos = G.vertices[next_food_id].vertex;
                line( frame2, food_pos, next_food_pos, Scalar(0,255,0), 4, 8);
            }
            for(int i=0;i<foods.size()-1;i++)
            {
                int food_id = foods[i];
                int next_food_id = foods[i+1];
                Point2f food_pos = G.vertices[food_id].vertex;
                Point2f next_food_pos = G.vertices[next_food_id].vertex;
                putText(frame2, to_string(i), food_pos, FONT_HERSHEY_SIMPLEX, 0.7, Scalar(0,0,0), 1, 8);
            }
            
            // for(int i=0;i<optimal_sequence.size()-1;i++)
            // {
            //     int food_id = optimal_sequence[i];
            //     int next_food_id = optimal_sequence[i+1];
            //     Point2f food_pos = G.vertices[food_id].vertex;
            //     Point2f next_food_pos = G.vertices[next_food_id].vertex;
            //     line( frame3, food_pos, next_food_pos, Scalar(0,255,0), 4, 8);
            // }
            for(int i=0;i<optimal_sequence.size()-1;i++)
            {
                int food_id = optimal_sequence[i];
                int next_food_id = optimal_sequence[i+1];
                Point2f food_pos = G.vertices[food_id].vertex;
                Point2f next_food_pos = G.vertices[next_food_id].vertex;
                putText(frame3, to_string(i), food_pos, FONT_HERSHEY_SIMPLEX, 0.7, Scalar(0,0,0), 1, 8);
            }

            // actual path planning for optimal_sequence
            int animation_route = r * optimal_sequence.size();
            for(int i=0;i<optimal_sequence.size()-1;i++)
            {
                int food_id = optimal_sequence[i];
                int next_food_id = optimal_sequence[i+1];
                vector<Vertex*> path;
                float path_length = find_path_in_graph(G, food_id, next_food_id, path);
                // draw path
                for(int j=0;j<path.size()-1;j++)
                {
                    Point2f p1 = path[j]->vertex;
                    Point2f p2 = path[j+1]->vertex;
                    if(i!=animation_route)
                        line( frame3, p1, p2, Scalar(0,100,0), 2, 8);
                    else
                        line( frame3, p1, p2, Scalar(0,0,225), 6, 8);
                }
            }

            // draw vertexs in graph
            for( int i = 0; i < G.vertices.size(); i++ )
            {
                if(G.vertices[i].food)
                {
                    circle( frame2, G.vertices[i].vertex, 7, Scalar(100,100,100), -1, 8);
                    circle( frame3, G.vertices[i].vertex, 7, Scalar(100,100,100), -1, 8);
                    circle( frame2, G.vertices[i].vertex, 5, Scalar(0,200,0), -1, 8);
                    circle( frame3, G.vertices[i].vertex, 5, Scalar(0,200,0), -1, 8);
                }
                else
                {
                    circle( frame2, G.vertices[i].vertex, 5, Scalar(0,0,255), -1, 8);
                }
            }

            // draw max_reward on frame3
            putText(frame3, "max reward : ", Point2f(50,50), FONT_HERSHEY_SIMPLEX, 1, Scalar(255,255,255), 6, 8);
            putText(frame3, "max reward : ", Point2f(50,50), FONT_HERSHEY_SIMPLEX, 1, Scalar(0,0,0), 2, 8);

            #ifdef HAS_FOOD
            double discount_factor = 1;
            #else
            double discount_factor = (50.0/69.0);
            #endif

            putText(frame3, to_string(max_reward*discount_factor), Point2f(270,50), FONT_HERSHEY_SIMPLEX, 1, Scalar(255,255,255), 4, 8);
            putText(frame3, to_string(max_reward*discount_factor), Point2f(270,50), FONT_HERSHEY_SIMPLEX, 1, Scalar(0,0,0), 2, 8);

            cv::imshow("frame", frame2);
            cv::imshow("best order", frame3);
            cv::waitKey(1);
            
        }
    }
    
    return 0;
}